/*
 * library.h
 *
 *  Created on: Oct 22, 2010
 *      Author: sanjeev
 */

#include <constants.h>

/** @param qij = value at (xi,yj)
 *  @param x,y	the position where you want the value
 *  @brief x1 < x2 and y1 < y2 so (x1,y1) is bottom left of (x,y)
 * 	and similarly (x1,y2) is top lft, (x2,y1) is bottom right
 */

double interpolate_at(double x, double y,
	double x1, double x2, double y1, double y2,
	double q11, double q12, double q21, double q22 );

double get_yaw(geometry_msgs::Quaternion qtr);
double get_yaw(tf::StampedTransform & transform);

double wave_front_marcher(VECTOR &v, double F);

PII index(double x,double y,double grain);

DD unindex(int x,int y, double grain);
